

/* Controller Logic for Remote Control on Arduino for a car */

/*  This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.
 
 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.
 
 You should have received a copy of the GNU General Public License
 along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 */

//#include "TimerOne.h"

//set the baudrate
const int baudRate = 9600;



enum COMMS_STATE{
    CONNECTED,
    DISCONNECTED
};



/*
 ///////////////////
 // HDLC Packet (Frame) parameters
 // All data is to be sent as ACSCII characters
 // Packet Structure
 //  - start flag
 //  - command
 //  - Payload length
 //  - Payload
 //  - CRC bytes (length = 2)
 //  - End Flag
 //////////////////
 */

const int BUFFER_LIMIT = 32; // max packet size

enum PACKET_DETAILS{ //positions excluding start
    CMD_LOC = 0,
    PKT_LEN_LOC,
    PAYLOAD_START_LOC
};

enum packetState { //Where is the processing of the packet state at
    PACKETSTART,
    COMMAND,
    SIZE,
    PAYLOAD,
    CKSUM,      //single byte - not implemented. Set to '00'
    PACKETEND
};

const byte STARTFLAG = (byte)'*'; //packet start marker
const byte ENDFLAG = (byte)'#'; //packet end marker

volatile enum packetState pktState;
volatile unsigned int payloadLength;


//setup the command list
enum COMMANDS {
    THROTTLE = 'a',
    STEERING,
    STATUS,            //Used to report status back via Serial
    WHEEL_RPM,
    CAR_VOLTAGE //voltage of Car Battery
};
/*
 //Known packet error conditions
 enum PACKET_STATUS{
 VALID_PKT,
 INCOMPLETE_PKT,
 BYTE_TIMEOUT,
 COMMS_LOST,
 MSG_INVALID,
 CMD_INVALID,
 THROTTLE_SIGN_INVALID,
 THROTTLE_AMNT_INVALID,
 STEERING_SIGN_INVALID,
 STEERING_AMNT_INVALID
 };
 */

volatile unsigned long invalidPacketCount = 0;
//this is the data packet structure.
//This is where we will keep all the necessary data fo keep track of the
//incoming data.
struct
{
    byte data[BUFFER_LIMIT]; //this is where we will store the data we receive from the serial
    byte curLoc; //this is the counter to keep track of how many bytes we've received
}
dataPacket;

//this tracks the state of the current message
bool correctPacket = false;

//this is used for the timeout
//the arduino will reset its packet counter if it
//finds a gap of over set amout between incoming bytes.
//const int byteTimeOutVal = 50; //byte timeout value in milliseconds (max threshold between bytes in a single packet)
volatile unsigned long lastTimerHit; //iast time byte was received
volatile unsigned long lastCommsLostSent; //last time Comms Lost Msg was sent
const int PKTTIMEOUT= 450; //threshold betweed valid packets before throttle is set to zero
//const int COMMSLOSTINTERVAL= 1000; //Only Report Comms Lost status if it hasn't been reported in the last millis in this const

const int DataSendInterval = 1000; //this is how often non heartbeat telemetry information is to be sent
volatile unsigned long lastCommsDataSent = 0; //time last telemetry data was sent

volatile unsigned long lastMsgHit; //last time valid Packet was received

//Throttle PWM pins
const int fwdThrottlePin = 11;
const int RvseThrottlePin = 10;

//Steering digital pins
const int rightSteeringPin = 5;
const int leftSteeringPin = 6;

//Status Pins
const int badCommsIndPin = 4;
const int goodCommsIndPin = 8;

//VoltageSensorPin
const int carVoltageSensorPin = 0; //Analog Pin 0

//Car Parameters
volatile float throttleAmnt = 0;
volatile float steeringAmnt = 0;

//RPM reading parameters
volatile int RPMTriggerCount = 0; //No of times RPM interrupts were triggered

//Only read RPM interrupts if the interval between interrupts is greater.
//To avoid sensor reading issues where multiple interrupts per transition are recorded.
const int MinRPMInterruptInterval = 5;
volatile unsigned long lastRPMInterruptTime;

//volatile unsigned long RPMCountTotal; //Total of counts in array
volatile unsigned long lastRPMTime; //last time RPM calculation was performed
//volatile int RPMCountArrayIndex; //index at which the next count is to be read into
const int RPMinterruptPin = 0; // Interrupt 0 on Pin 2
const int RPMInterruptPerRotation = 1; //no. of reflectors per rotation on wheel
//const int TimerInterruptInterval = 500; //how often will timer interrupt for RPM count - in milliseconds

//debug switch
bool debug = false;

int status = DISCONNECTED; //set to disconnected at start

void setup()
{
    //initialise 'packet' variables
    dataPacket.curLoc = 0;
    pktState = PACKETSTART;
    
    correctPacket = false;
    lastTimerHit = 0;
    lastMsgHit = 0;
    lastCommsLostSent = 0;
    
    // Set up the throttle and steerings pisn to be an output:
    
    pinMode(fwdThrottlePin, OUTPUT);
    pinMode(RvseThrottlePin, OUTPUT);
    pinMode(rightSteeringPin, OUTPUT);
    pinMode(leftSteeringPin, OUTPUT);
    
    pinMode(badCommsIndPin, OUTPUT);
    pinMode(goodCommsIndPin, OUTPUT);
    
    // Set up the serial port:
    
    Serial.begin(baudRate);
    
    //Initialiase RPM parameters
    RPMTriggerCount = 0;
    
    attachInterrupt(RPMinterruptPin, UpdateRPMCount, FALLING);
    lastRPMTime = 0;
    
}



void loop()
{
    
    readIncomingSerial(); //read input and interpret
    
    //Failsafe - if time between valid packets exceed threshold then set throttle to zero
    if ( (lastMsgHit + PKTTIMEOUT) < millis() )
    {
        if (debug) Serial.print("lastMsgHit: ");
        if (debug) Serial.println(lastMsgHit);
        if (debug) Serial.print("Current Millis: ");
        if (debug) Serial.println(millis());
        
        status = DISCONNECTED;
        digitalWrite(badCommsIndPin, HIGH);
        digitalWrite(goodCommsIndPin, LOW);
        
        throttleAmnt = 0;
        /*
         if ( (lastCommsLostSent + COMMSLOSTINTERVAL) < millis() ){ //To prevent from sending this too often and saturating serial comms
         
         RptStatus(COMMS_LOST);
         lastCommsLostSent = millis();
         if (debug) Serial.println("COMMS LOST Sent"); //Testing only
         }
         */
    }
    else{
        status = CONNECTED; //valid heartbeat received within threshold
        digitalWrite(badCommsIndPin, LOW);
        digitalWrite(goodCommsIndPin, HIGH);
    }
    
    
    setThrottle(throttleAmnt); //apply required throttle
    
    setSteering(steeringAmnt);
    
    //do we need to send telemetry data
    if ((millis() - lastCommsDataSent) > DataSendInterval)
        sendTelemetryData();
    
    
}





void processCommand(byte buff[], int dataLength){
    //This function processes the command received in the HDLC packet. It is called when a valid packet is received.
//    Serial.println("Processing Command");
    
    //check the first char of the buffer for command
    switch (buff[CMD_LOC])
    {
            
            
            //Handle Throttle Command
        case THROTTLE:
//            Serial.print("dataLength: ");
//            Serial.println(dataLength);
            
            //get throttle amount from array. first byte of sign and rest is absolute amount in buffer.
//            Serial.println("Throttle Command");
            int size;
            size = dataLength-1;
            byte throttleVals[9]; //hardcoding size here......else weird behaviour (crash?) observed....
            
            for (int q = 1; q<dataLength; q++) { // starting at 1 as we don't want the sign byte
                throttleVals[q-1] = buff[PAYLOAD_START_LOC+q];
            }
            int throttleInput;
            throttleInput = intFromByteArray(throttleVals, (dataLength-1)); //length-1 due to removal of sign byte
            
            
            if ((throttleInput >= 0) && (throttleInput <= 100)){
                if (buff[PAYLOAD_START_LOC] == '+' ){ // positive throttle
                    throttleAmnt = throttleInput;
                }
                else if (buff[PAYLOAD_START_LOC] == '-' ){ // negative throttle - reverse
                    throttleAmnt = -1 * throttleInput;
                }
                else {//invalid packet
//                    Serial.println("Invalid Throttle sign"); //Testing only
                    invalidPacketCount++;
                }
            }
            else{//invalid throttle amount. Discard packet
//                Serial.print("Invalid Throttle amount:");
//                Serial.println(throttleInput);
                invalidPacketCount++;
            }
//            Serial.print("throttleAmnt: ");
//            Serial.println(throttleAmnt);
            
            break;
            
            
            //Handle Steering Command
        case STEERING:
            
            //get throttle amount from array. first byte of sign and rest is absolute amount in buffer.
            byte steeringVals[9];
            
            for (int q = 1; q<dataLength; q++) { // starting at 1 as we don't want the sign byte
                steeringVals[q-1] = buff[PAYLOAD_START_LOC+q];
            }
            int steeringInput;
            steeringInput = intFromByteArray(steeringVals, (dataLength-1));
            
            if ((steeringInput >= 0) && (steeringInput <= 100)){
                if (buff[PAYLOAD_START_LOC] == '+' ){ // positive throttle
                    steeringAmnt = steeringInput;
                }
                else if (buff[PAYLOAD_START_LOC] == '-' ){ // negative throttle - reverse
                    steeringAmnt = -1 * steeringInput;
                }
                else {//invalid packet
//                    Serial.println("Invalid Steering sign"); //Testing only
                    invalidPacketCount++;
                }
            }
            else{//invalid throttle amount. Discard packet
                
                invalidPacketCount++;
            }
//            Serial.print("steeringAmnt: ");
//            Serial.println(steeringAmnt);
            
            break;
            
        default:
            
            //if here, the command byte is wrong
            
            invalidPacketCount++;
            
//            Serial.println("Invalid Command");
            break;
    }
    
    //After processing the packet send heartbeat (with RPM data)
    //Serial.println("Calling RPM Reporter"); //Testing only
    RptRPM();
    
    
}




void setThrottle(int throttle)
{
    
    throttle = constrain(throttle, -100, 100);
    // fwd or reverse?
    
    if (throttle >=0){
        
        //set the speed of the motor
        if (debug) Serial.print("Setting Fwd throttle: ");
        if (debug) Serial.println((throttle/100.0) * 255.0);
        analogWrite(fwdThrottlePin, (throttle/100.0) * 255.0);
        analogWrite(RvseThrottlePin, 0);
    }
    else{
        
        throttle = -1 * throttle;
        if (debug) Serial.print("Setting Reverse throttle: ");
        if (debug) Serial.println((throttle/100.0) * 255.0);
        analogWrite(RvseThrottlePin, (throttle/100.0) * 255.0 );
        analogWrite(fwdThrottlePin, 0);
        
    }
}

void setSteering(int steering) //Negative Value = left turn
{
    
    steering = constrain(steering, -100, 100);
    // fwd or reverse?
    
    if (steering >=0){
        
        //set the speed of the motor
        if (debug) Serial.print("Setting Right Steering : ");
        if (debug) Serial.println(steering);
        analogWrite(rightSteeringPin, (steering/100.0) * 255.0 );
        analogWrite(leftSteeringPin, 0);
    }
    else{
        
        steering = -1 * steering;
        if (debug) Serial.print("Setting Left Steering : ");
        if (debug) Serial.println(steering);
        analogWrite(leftSteeringPin, (steering/100.0) * 255.0);
        analogWrite(rightSteeringPin, 0);
        
    }
    
}

void UpdateRPMCount(){
    
    //this function is called by the RPM interrupt. It updates the counter of triggers
    //To avoid false sensor readings, ignore interrupts that are too close together
    if ((millis() - lastRPMInterruptTime) > MinRPMInterruptInterval)
        RPMTriggerCount++;
    lastRPMInterruptTime = millis();
    
    //Beware - doing serial writes in an interrupt routine may cause Arduino to freeze
    //  Serial.print("Count :");
    //  Serial.println(RPMTriggerCount);
    
}


void RptRPM(){
//    Serial.println("Reporting RPM");
    //This function calculates the RPM and sends to controller
    //stop interrup and take a local copy
    noInterrupts();
    int RPMTotalCopy = RPMTriggerCount;
    RPMTriggerCount = 0;
    
    interrupts();
    
    /*  Serial.print("Time: ");
     Serial.print(millis());
     Serial.print("lastRPMTime: ");
     Serial.print(lastRPMTime);
     
     Serial.print(" Tot :");
     
     Serial.println(RPMTotalCopy);
     */
    
    
    
    float RPM = 60.0*1000.0/((float)millis() - (float)lastRPMTime)*(float)RPMTotalCopy;
    
    //constuct value to be sent. RPM value to be sent in 16 bits in format 00000.0 (without the decimal point)
    int RPM10 = 10 * RPM;
    
    lastRPMTime = millis();
    
    //  Serial.print("RPM10 : ");
    //  Serial.println(RPM10);
    
    
    //send message
    byte payLoadData[BUFFER_LIMIT];
    int dataSize;
    dataSize = intToByteArray(payLoadData, RPM10); //updates buffer and return size
    sendPacket(WHEEL_RPM, dataSize, payLoadData);
    
    //Serial.write(WHEEL_RPM); //Command character in packet
    //  Serial.print('d');
    
}



void sendTelemetryData(){
    
    //Read Car Battery Voltage and Report. Multiply by two due to voltage divider at sensor
    float carVolts = 2.0 * (float)analogRead(carVoltageSensorPin)/1023.0*5.0;
    
    //multiply by 100 to get two decimal points and send as two byte integer. To be divided by 100 at receiver
    int carVolts100 = 100 * carVolts;
    //    Serial.write(CAR_VOLTAGE);
    //    send2ByteVal(carVolts100);
    byte payLoadData[BUFFER_LIMIT];
    int dataSize;
    dataSize = intToByteArray(payLoadData, carVolts100); //updates buffer and return size
    sendPacket(CAR_VOLTAGE, dataSize, payLoadData);
    
    lastCommsDataSent = millis();
}

///////////////////////////////////////////
//
//				HDLC Methods
//
///////////////////////////////////////////

void readIncomingSerial()
//This function reads the serial buffer and constructs the HDLC packet. Once a valid packet is found it dispatches it for processing.
//Any invalid packets are discarded.
{
    
    
    while (Serial.available() > 0) {
        
        byte byteIn = Serial.read();
        //    Serial.print("Read in: ");
        //   Serial.println(byteIn);
        if (byteIn == STARTFLAG){
            if (pktState != PACKETSTART){
                //        Serial.println("Discarded Last Packet");
                invalidPacketCount++;
            }
            pktState = COMMAND; //reset packet and get ready to read next byte
            dataPacket.curLoc = 0;
            //      Serial.println("Starting Packet....");
            
        }
        else{
            //Serial.println(dataPacket.data[dataPacket.curLoc]);
            //     Serial.print("dataPacket.curLoc: ");
            //     Serial.println(dataPacket.curLoc);
            dataPacket.data[dataPacket.curLoc] = byteIn;
            
            
            switch (pktState){
                case COMMAND:
                    //        Serial.print("Command: ");
                    //        Serial.println(byteIn);
                    pktState = SIZE; // next byte
                    break;
                    
                case SIZE:
                    payloadLength = byteIn - '0';
                    if (payloadLength > 0)
                        pktState = PAYLOAD;
                    else
                        pktState = CKSUM;
                    
                    //        Serial.print("Payload Size: ");
                    //        Serial.println(payloadLength);
                    break;
                    
                case PAYLOAD:
                    if (dataPacket.curLoc == (payloadLength + 1)) {
                        pktState = CKSUM;
                    }
                    //        Serial.print("Payload byte: ");
                    //        Serial.println(byteIn);
                    break;
                    
                case CKSUM:
                    //    if (calc_CRC(dataPacket.data) == (byteIn = '0')) {
                    //        Serial.print("CKSUM: ");
                    //        Serial.println(byteIn);
                    
                    //        Serial.print("calc_CRC(): ");
                    //        Serial.println(calc_CRC());
                    //        Serial.print("(byte)'0': ");
                    //        Serial.println((byte)'0');
                    if (calc_CRC() == (byteIn - (byte)'0')) {
                        
                        correctPacket = true;
                        pktState = PACKETEND;
                        //          Serial.println("Checksum OK");
                        
                    }
                    else{
                        pktState = PACKETSTART; //discard packet
                        dataPacket.curLoc =0;
                        //          Serial.println("Invalid Checksum ");
                        invalidPacketCount++;
                    }
                    
                    
                    break;
                    
                case PACKETEND:
                    //        Serial.print("Packet End: ");
                    //        Serial.println(byteIn);
                    //        Serial.print("ENDFLAG: ");
                    //        Serial.println(ENDFLAG);
                    //       Serial.print("correctPacket: ");
                    //        Serial.println(correctPacket);
                    if ((byteIn == ENDFLAG) && correctPacket) {
                        
 /*                       
                        Serial.print("Correct Message Received: ");
                        for (int i=0; i<=dataPacket.curLoc;i++)
                            Serial.print(char(dataPacket.data[i]));
                        Serial.print("  At Time: ");
                        Serial.println(millis());
                        
                        
  */                      
                        processCommand(dataPacket.data, payloadLength);
                        lastMsgHit = millis();
                        
                        
                        pktState = PACKETSTART;
                        dataPacket.curLoc = 0;
                        
                        
                        
                        
                    }
                    else{ //discard packet as packet terminator is not in expected position
                        pktState = PACKETSTART;
                        dataPacket.curLoc = 0;
                        correctPacket = false;
                        invalidPacketCount++;
                    }
                    break;
                    
            }
            
            dataPacket.curLoc++;
        }
        
        
    }
}


//short calc_CRC(byte (buff))[BUFFER_LIMIT]){
short calc_CRC(){
    
    //not implemented properly due to short packet size. setting return to 0 always
    return 0;
}

void sendPacket(byte cmd, int dataLen, byte payLoadData[]){
    //sends the HDLC packet via Serial
    Serial.print((char)STARTFLAG);
    Serial.print((char)cmd);
    for (int i = 0; i < (dataLen+1); i++) //dataLength+1 as the length byte is included in buffer
        Serial.print((char)payLoadData[i]); //includes data length in first byte
    //   Serial.print(calc_CRC);
    Serial.print('0');
    
    Serial.print((char)ENDFLAG);
}


////////////////////////////////////////////////////////////
//
//      Byte to Value / Value to Byte conversion methods
//
//////////////////////////////////////////////////////////




int intFromByteArray(byte byteData[], int dataLength){
    //this function calculates the integer from the ASCII 'char' values in the byte buffer
    
    int val = 0;
    int array_mult = 1;
    //    Serial.print("intFromByteArray - dataLength: ");
    //    Serial.println(dataLength);
    
    //    Serial.print("intFromByteArray - byteData: ");
    //    for (int s=0;s<dataLength;s++)
    //        Serial.print((char)byteData[s]);
    
    //    Serial.println("");
    for(int k = (dataLength-1); k > -1 ; k--) {
        //        Serial.print("byteData[k] - '0': ");
        //        Serial.println(byteData[k] - '0');
        //        Serial.print("array_mult: ");
        //        Serial.println(array_mult);
        val = val + (byteData[k] - '0') * array_mult;
        //        Serial.print("val: ");
        //        Serial.println(val);
        array_mult =  array_mult* 10;
    }
    
    //    Serial.print("intFromByteArray: ");
    //    Serial.println(val);
    return val;
}

int intToByteArray(byte *buff, int Val){
    //this function constructs a char (ASCII) array for a given integer (up to 9 digits). It places the size of the byte array in the initial position
    // Required to support the HDL protocol
    //returns the actual size required
    
    //    Serial.print("intToByteArray - Val:");
    //    Serial.println(Val);
    
    int size; //how many bytes are actually need to transfer data
    char tmpBuffer[9];
    //    memcpy(buff, &Val, 9 );
    size = sprintf(tmpBuffer, "%d", Val); //determine size of payload and populate data
    
    
    // Serial.print("temp Buffer: ");
    //for (int w = 0; w< size; w++) {
    //    Serial.print((char)tmpBuffer[w]);
    //}
    //Serial.println();
    
    //Set up output buffer
    buff[0] = size + '0'; //set the size in char format
    
    //Serial.print("Size:");
    //Serial.println(size);
    
    //populate the rest of the buffer
    for (int p = 1; p< (size+1); p++) {
        //        buff[p] = tmpBuffer[8-size+p];
        buff[p] = tmpBuffer[p-1];
    }
    /*
     Serial.print("Buffer: ");
     for (int q = 0; q< (size+1); q++) {
     Serial.print((char)buff[q]);
     }
     
     Serial.println();
     */
    return size;
}








